#include "coordinate_transformer.h"

void set_rotation_x(Matrix3d *R, double angle_deg)
{
    assert(R != NULL);
    double angle_rad = angle_deg * DEG2RAD;
    double c = cos(angle_rad), s = sin(angle_rad);
    R->data[0][0] = 1.0;
    R->data[0][1] = 0.0;
    R->data[0][2] = 0.0;
    R->data[1][0] = 0.0;
    R->data[1][1] = c;
    R->data[1][2] = -s;
    R->data[2][0] = 0.0;
    R->data[2][1] = s;
    R->data[2][2] = c;
}

void set_rotation_y(Matrix3d *R, double angle_deg)
{
    assert(R != NULL);
    double angle_rad = angle_deg * DEG2RAD;
    double c = cos(angle_rad), s = sin(angle_rad);
    R->data[0][0] = c;
    R->data[0][1] = 0.0;
    R->data[0][2] = s;
    R->data[1][0] = 0.0;
    R->data[1][1] = 1.0;
    R->data[1][2] = 0.0;
    R->data[2][0] = -s;
    R->data[2][1] = 0.0;
    R->data[2][2] = c;
}

void set_rotation_z(Matrix3d *R, double angle_deg)
{
    assert(R != NULL);
    double angle_rad = angle_deg * DEG2RAD;
    double c = cos(angle_rad), s = sin(angle_rad);
    R->data[0][0] = c;
    R->data[0][1] = -s;
    R->data[0][2] = 0.0;
    R->data[1][0] = s;
    R->data[1][1] = c;
    R->data[1][2] = 0.0;
    R->data[2][0] = 0.0;
    R->data[2][1] = 0.0;
    R->data[2][2] = 1.0;
}

void set_rotation_ecef_to_ned(Matrix3d *R, double lon_deg, double lat_deg)
{
    assert(R != NULL);
    double lon_rad = lon_deg * DEG2RAD;
    double lat_rad = lat_deg * DEG2RAD;

    double sin_lng = sin(lon_rad), cos_lng = cos(lon_rad);
    double sin_lat = sin(lat_rad), cos_lat = cos(lat_rad);

    R->data[0][0] = -sin_lat * cos_lng;
    R->data[0][1] = -sin_lat * sin_lng;
    R->data[0][2] = cos_lat;

    R->data[1][0] = -sin_lng;
    R->data[1][1] = cos_lng;
    R->data[1][2] = 0.0;

    R->data[2][0] = -cos_lat * cos_lng;
    R->data[2][1] = -cos_lat * sin_lng;
    R->data[2][2] = -sin_lat;
}

void set_rotation_ned_to_ecef(Matrix3d *R, double lon_deg, double lat_deg)
{
    assert(R != NULL);
    // ECEF->NED矩阵的转置
    Matrix3d tmp;
    set_rotation_ecef_to_ned(&tmp, lon_deg, lat_deg);
    matrix_transpose_3x3(&tmp, R);
}

void set_rotation_ned_to_body(const Aircraft *aircraft, Matrix3d *rotation_matrix)
{
    assert(aircraft != NULL && rotation_matrix != NULL);

    Matrix3d Rz_yaw, Ry_pitch, Rx_roll;
    set_rotation_x(&Rx_roll, aircraft->roll);
    set_rotation_y(&Ry_pitch, aircraft->pitch);
    set_rotation_z(&Rz_yaw, aircraft->yaw);

    // yaw→pitch→roll主动旋转+内旋 R_ned_to_body = Rz*Ry*Rx
    Matrix3d R_tmp;
    matrix_multiply_3x3(&Ry_pitch, &Rx_roll, &R_tmp);      // Ry * Rx
    matrix_multiply_3x3(&Rz_yaw, &R_tmp, rotation_matrix); // Rz * (Ry * Rx)
}

void set_rotation_body_to_ned(const Aircraft *aircraft, Matrix3d *rotation_matrix)
{
    assert(aircraft != NULL && rotation_matrix != NULL);

    Matrix3d Rz_yaw, Ry_pitch, Rx_roll;
    set_rotation_x(&Rx_roll, -aircraft->roll);
    set_rotation_y(&Ry_pitch, -aircraft->pitch);
    set_rotation_z(&Rz_yaw, -aircraft->yaw);

    // R_body_to_ned = Rx(-)*Ry(-)*Rz(-) = R_ned_to_body^T
    Matrix3d R_tmp;
    matrix_multiply_3x3(&Ry_pitch, &Rz_yaw, &R_tmp);        // Ry(-) * Rz(-)
    matrix_multiply_3x3(&Rx_roll, &R_tmp, rotation_matrix); // Rx(-) * (Ry(-) * Rz(-))
}

void set_rotation_body_to_camera(const Camera_Orentation *camera_orenation, Matrix3d *rotation_matrix)
{
    assert(camera_orenation != NULL && rotation_matrix != NULL);

    // 分别绕Z Y X轴旋转
    Matrix3d Rz_yaw, Ry_pitch, Rx_roll;
    set_rotation_x(&Rx_roll, camera_orenation->roll);
    set_rotation_y(&Ry_pitch, camera_orenation->pitch);
    set_rotation_z(&Rz_yaw, camera_orenation->yaw);
    // R_body_to_camera = Rz*Ry*Rx
    Matrix3d R_tmp;
    matrix_multiply_3x3(&Ry_pitch, &Rx_roll, &R_tmp);      // Ry * Rx
    matrix_multiply_3x3(&Rz_yaw, &R_tmp, rotation_matrix); // Rz * (Ry * Rx)
}

void set_rotation_camera_to_body(const Camera_Orentation *camera_orenation, Matrix3d *rotation_matrix)
{
    Matrix3d Rz_yaw, Ry_pitch, Rx_roll;
    set_rotation_x(&Rx_roll, -camera_orenation->roll);
    set_rotation_y(&Ry_pitch, -camera_orenation->pitch);
    set_rotation_z(&Rz_yaw, -camera_orenation->yaw);

    // R_camera_to_body = Rx(-)*Ry(-)*Rz(-) = R_body_to_camera^T
    Matrix3d R_tmp;
    matrix_multiply_3x3(&Ry_pitch, &Rz_yaw, &R_tmp);        // Ry(-) * Rz(-)
    matrix_multiply_3x3(&Rx_roll, &R_tmp, rotation_matrix); // Rx(-) * (Ry(-) * Rz(-))
}

void lla_to_ecef(const Vector3d *lla, Vector3d *ecef)
{
    assert(ecef != NULL);

    // 经纬高
    double lon_rad = lla->x * DEG2RAD;
    double lat_rad = lla->y * DEG2RAD;
    double alt = lla->z;
    // 计算第一偏心率平方 e^2
    // const double e2 = EARTH_ECCENTRICITY * EARTH_ECCENTRICITY;
    const double e2 = 6.69437999014e-3;
    // 卯酉圈半径
    double N = EARTH_SEMI_MAJOR / sqrt(1.0 - e2 * sin(lat_rad) * sin(lat_rad));

    // 计算ECEF坐标
    ecef->x = (N + alt) * cos(lat_rad) * cos(lon_rad);
    ecef->y = (N + alt) * cos(lat_rad) * sin(lon_rad);
    ecef->z = (N * (1.0 - e2) + alt) * sin(lat_rad);
}

void ecef_to_lla(const Vector3d *ecef, Vector3d *lla)
{
    assert(ecef != NULL && lla != NULL);

    const double a = EARTH_SEMI_MAJOR;
    const double e2 = 6.69437999014e-3;

    double x = ecef->x, y = ecef->y, z = ecef->z;
    double p = sqrt(x * x + y * y);

    // 经度直接计算
    double lon_rad = atan2(y, x);

    // 使用迭代法计算纬度, 提高精度
    double lat_rad = atan2(z, p * (1.0 - e2));
    double N, alt, prev_alt;
    int iterations = 0;

    do
    {
        N = a / sqrt(1.0 - e2 * sin(lat_rad) * sin(lat_rad));
        prev_alt = alt;
        alt = p / cos(lat_rad) - N;
        lat_rad = atan2(z, p * (1.0 - e2 * (N / (N + alt))));
        iterations++;
    } while (fabs(alt - prev_alt) > 1e-9 && iterations < 10);

    lla->x = lon_rad * RAD2DEG;
    lla->y = lat_rad * RAD2DEG;
    lla->z = alt;

    // const double a = EARTH_SEMI_MAJOR;
    // const double b = EARTH_SEMI_MINOR;
    // const double e2 = EARTH_ECCENTRICITY * EARTH_ECCENTRICITY;
    // const double ep2 = EARTH_SECOND_ECCENTRICITY * EARTH_SECOND_ECCENTRICITY;

    // double x = ecef->x;
    // double y = ecef->y;
    // double z = ecef->z;

    // double p = sqrt(x * x + y * y);
    // // 极点处理
    // if (p < 1e-6)
    // {
    //     lla->x = 0.0;
    //     lla->y = (z >= 0.0) ? 90.0 : -90.0;
    //     lla->z = fabs(z) - b;
    //     return;
    // }
    // // 初始辅助角
    // double q = atan2(z * a, p * b);

    // // 经纬度(弧度)
    // double lon_rad = atan2(y, x);
    // double lat_rad = atan2(z + ep2 * b * pow(sin(q), 3), p - e2 * a * pow(cos(q), 3));

    // // 卯酉圈半径
    // double N = a / sqrt(1.0 - e2 * sin(lat_rad) * sin(lat_rad));

    // // 高度
    // double alt = (p / cos(lat_rad)) - N;

    // // 经度,纬度,高度(度,度,米)
    // lla->x = lon_rad * RAD2DEG;
    // lla->y = lat_rad * RAD2DEG;
    // lla->z = alt;
}

void ecef_to_ned(const Base_LLA *base_lla, const Vector3d *ecef, Vector3d *ned)
{
    assert(ecef != NULL && base_lla != NULL && ned != NULL);

    // ECEF->NED
    Matrix3d R_ecef2ned;
    set_rotation_ecef_to_ned(&R_ecef2ned, base_lla->base_lon, base_lla->base_lat);

    Vector3d base_lla_tmp;
    base_lla_tmp.x = base_lla->base_lon;
    base_lla_tmp.y = base_lla->base_lat;
    base_lla_tmp.z = base_lla->base_alt;
    // 基准点的LLA坐标->ECEF坐标
    Vector3d base_ecef;
    lla_to_ecef(&base_lla_tmp, &base_ecef);

    // 计算ECEf下的坐标差
    Vector3d bias_xyz = {ecef->x - base_ecef.x, ecef->y - base_ecef.y, ecef->z - base_ecef.z};

    // 转换到NED坐标系(偏差向量乘旋转矩阵)
    matrix3d_mul_vector3d(&R_ecef2ned, &bias_xyz, ned);
}

void ned_to_ecef(const Base_LLA *base_lla, const Vector3d *ned, Vector3d *ecef)
{
    assert(ned != NULL && ecef != NULL);

    double lon_deg = base_lla->base_lon;
    double lat_deg = base_lla->base_lat;
    double alt0 = base_lla->base_alt;

    // 转换矩阵R_ned2ecef,NED到ECEF
    Matrix3d R_ned2ecef;
    set_rotation_ned_to_ecef(&R_ned2ecef, lon_deg, lat_deg);

    double lat_rad = lat_deg * DEG2RAD;
    double lon_rad = lon_deg * DEG2RAD;
    double N = EARTH_SEMI_MAJOR / sqrt(1.0 - EARTH_ECCENTRICITY * EARTH_ECCENTRICITY * sin(lat_rad) * sin(lat_rad));

    // 基准点ECEF坐标
    Vector3d Pe0 = {
        (N + alt0) * cos(lat_rad) * cos(lon_rad),
        (N + alt0) * cos(lat_rad) * sin(lon_rad),
        (N * (1 - EARTH_ECCENTRICITY * EARTH_ECCENTRICITY) + alt0) * sin(lat_rad)};

    // 根据NED坐标转换回ECEF,ECEF坐标 = 基准点ECEF + 旋转后的NED偏移
    matrix3d_mul_vector3d(&R_ned2ecef, ned, ecef);
    vector3d_add(ecef, &Pe0, ecef);
}

void ned_to_body(const Vector3d *ned, const Aircraft *aircraft, Vector3d *body)
{
    assert(ned != NULL && aircraft != NULL && body != NULL);

    Matrix3d R_ned2body;
    set_rotation_ned_to_body(aircraft, &R_ned2body); // 得到世界坐标(NED)到机体坐标(BODY)的旋转矩阵
    matrix3d_mul_vector3d(&R_ned2body, ned, body);
}

void body_to_ned(const Vector3d *body, const Aircraft *aircraft, Vector3d *ned)
{
    assert(body != NULL && aircraft != NULL && ned != NULL);

    Matrix3d R_body2ned;
    set_rotation_body_to_ned(aircraft, &R_body2ned);
    matrix3d_mul_vector3d(&R_body2ned, body, ned);
}

void body_to_camera(const Vector3d *body, const Camera_Orentation *camera_orenation, Vector3d *camera)
{
    assert(body != NULL && camera_orenation != NULL && camera != NULL);

    Matrix3d R_body2camera;
    set_rotation_body_to_camera(camera_orenation, &R_body2camera); // 机体→相机的旋转矩阵
    matrix3d_mul_vector3d(&R_body2camera, body, camera);
}

void camera_to_body(const Vector3d *camera, const Camera_Orentation *camera_orenation, Vector3d *body)
{
    assert(camera != NULL && camera_orenation != NULL && body != NULL);

    Matrix3d R_camera2body;
    set_rotation_camera_to_body(camera_orenation, &R_camera2body);
    matrix3d_mul_vector3d(&R_camera2body, camera, body);
}

void lla_to_ned(const Base_LLA *base_lla, const Vector3d *lla, Vector3d *ned)
{
    assert(lla != NULL && base_lla != NULL && ned != NULL);
    // printf("lla: [%.8f, %.8f, %.2f]\n", lla->x, lla->y, lla->z);

    // Aircraft → ECEF
    Vector3d ecef;
    lla_to_ecef(lla, &ecef);
    // printf("ecef: [%.8f, %.8f, %.8f]\n", ecef.x, ecef.y, ecef.z);

    // ECEF → NED
    ecef_to_ned(base_lla, &ecef, ned);
    // printf("ned: [%.8f, %.8f, %.8f]\n", ned->x, ned->y, ned->z);
}

void ned_to_lla(const Base_LLA *base_lla, const Vector3d *ned, Vector3d *lla)
{
    assert(ned != NULL && base_lla != NULL && lla != NULL);
    // printf("ned: [%.8f, %.8f, %.2f]\n", ned->x, ned->y, ned->z);

    // NED -> ECEF
    Vector3d ecef;
    ned_to_ecef(base_lla, ned, &ecef);
    // printf("ecef: [%.8f, %.8f, %.2f]\n", ecef.x, ecef.y, ecef.z);

    // ECEF -> Aircraft
    ecef_to_lla(&ecef, lla);
    // printf("lla: [%.8f, %.8f, %.3f]\n", lla->x, lla->y, lla->z);
}

double cal_3d_distance(const Base_LLA *base_lla, const Vector3d *lla1, const Vector3d *lla2)
{
    Vector3d ned1, ned2;
    lla_to_ned(base_lla, lla1, &ned1);
    lla_to_ned(base_lla, lla2, &ned2);

    double dn = ned1.x - ned2.x; // ΔN
    double de = ned1.y - ned2.y; // ΔE
    double dd = ned1.z - ned2.z; // ΔD

    return sqrt(dn * dn + de * de + dd * dd);
}

double cal_horizontal_distance(const Base_LLA *base_lla, const Vector3d *lla1, const Vector3d *lla2)
{
    Vector3d ned1, ned2;
    lla_to_ned(base_lla, lla1, &ned1);
    lla_to_ned(base_lla, lla2, &ned2);

    double dn = ned1.x - ned2.x; // ΔN
    double de = ned1.y - ned2.y; // ΔE

    return sqrt(dn * dn + de * de);
}

double cal_bearing(const Base_LLA *base_lla, const Vector3d *lla1, const Vector3d *lla2)
{
    Vector3d ned1, ned2;
    lla_to_ned(base_lla, lla1, &ned1);
    lla_to_ned(base_lla, lla2, &ned2);

    double dn = ned2.x - ned1.x; // ΔN = N2 - N1
    double de = ned2.y - ned1.y; // ΔE = E2 - E1

    // 处理水平距离为零的情况
    if (fabs(dn) < 1e-9 && fabs(de) < 1e-9)
    {
        return 0.0; // 或者返回一个特殊值, 如-1, 表示未定义
    }

    // 使用atan2计算方位角(弧度)
    // atan2(de, dn) 返回从北方向顺时针的角度
    double bearing_rad = atan2(de, dn);

    // 将弧度转换为度数
    double bearing_deg = bearing_rad * 180.0 / M_PI;

    // 调整到0-360度范围
    if (bearing_deg < 0)
    {
        bearing_deg += 360.0;
    }
    if (bearing_deg >= 360.0)
    {
        bearing_deg -= 360.0;
    }

    return bearing_deg;
}

double cal_elevation(const Base_LLA *base_lla, const Vector3d *lla1, const Vector3d *lla2)
{
    Vector3d ned1, ned2;
    lla_to_ned(base_lla, lla1, &ned1);
    lla_to_ned(base_lla, lla2, &ned2);

    double dn = ned2.x - ned1.x;
    double de = ned2.y - ned1.y;
    double dd = ned2.z - ned1.z;

    double horizontal_dist = sqrt(dn * dn + de * de);
    double elevation_rad = atan2(-dd, horizontal_dist); // D向下为正, 所以用-dd

    return elevation_rad * 180.0 / M_PI;
}